#include "world.h"
#include "applications/adas_application.h"

#include "remote_vehicle.h"

using namespace decision::adas;

World::World()
    : rclcpp::Node("DecisionWorld")
    , logger_(rclcpp::get_logger("DecisionWorld"))
{
}

void World::build()
{
    pub_ = this->create_publisher<interface::msg::Event>("/decision/event", 10);
    sub_ = this->create_subscription<interface::msg::Objects>("/perception/multiFused/objects", 10,
        std::bind(&World::onReceiveNerghberData, this, std::placeholders::_1));
    {
        this->declare_parameter<double>("host_length", 5);
        this->declare_parameter<double>("host_width", 2);
        this->declare_parameter<double>("host_ydiff", 0.5);

        double length = 5;
        double width = 2;
        double ydiff = 0.5;
        this->get_parameter("host_length", length);
        this->get_parameter("host_width", width);
        this->get_parameter("host_ydiff", ydiff);

        host_ = std::make_shared<Vehicle>(-1);
        host_->update(0, 0, M_PI_2, length, width);

        RCLCPP_INFO(logger_, "Host Info:%s", host_->debug_string().c_str());
    }

    {
        std::unique_lock<std::mutex> guard;
        neighbers_.mutable_content(guard);
    }
}

void World::start()
{
    for (auto& app : applications_) {
        app.second->start();
    }
}

void World::report(const interface::msg::Event& ev)
{
    RCLCPP_INFO(logger_, "**Report event %s,%d, tid:%ld", ev.type.c_str(), ev.level, ev.vehicles.back().trackid);
    pub_->publish(ev);
}

std::shared_ptr<const Vehicle> World::get_host_vehicle()
{
    return host_;
}

std::shared_ptr<const std::map<int64_t, std::shared_ptr<RemoteVehicle>>> World::get_neighbers()
{
    return neighbers_.content();
}

void World::onReceiveNerghberData(const interface::msg::Objects::SharedPtr objects)
{
    RCLCPP_INFO(logger_, "onReceiveNerghberData objects num: %lu", objects->objects.size());
    std::cout << " ---------------------------------- " << std::endl;
    std::map<int64_t, std::shared_ptr<RemoteVehicle>> updated_neighbers;

    { // 只读neighbers_
        auto neighbers = neighbers_.content();
        RCLCPP_INFO(logger_, "Host Info:%s", host_->debug_string().c_str());
        for (auto& obj : objects->objects) {
            auto iter = neighbers->find(obj.id);
            auto neighber = (iter == neighbers->end()) ? std::make_shared<RemoteVehicle>(obj.id) : iter->second;
            neighber->update(obj.x, obj.y, M_PI_2 - (obj.heading) / 180 * M_PI, obj.length, obj.width);
            RCLCPP_INFO(logger_, "Neighber Info:%s", neighber->debug_string().c_str());
            neighber->calc_relation(*host_);
            RCLCPP_INFO(logger_, "Neighber Relation:%s", neighber->relation_debug_string().c_str());
            updated_neighbers[obj.id] = neighber;
        }

        for (auto neighber : *neighbers) {
            if (updated_neighbers.find(neighber.first) != updated_neighbers.end()) {
                continue;
            }

            if (muduo::timeDifference(muduo::Timestamp::now(), neighber.second->get_update_time()) > 2) { // 忽略2s未更新的数据
                continue;
            }

            updated_neighbers[neighber.first] = neighber.second; // 继续缓存2s内未更新的数据
        }
    }

    { // 改写neighbers_
        std::unique_lock<std::mutex> guard;
        auto neighbers = neighbers_.mutable_content(guard);
        neighbers->swap(updated_neighbers);
    }
}
